#include "cybertron/sensor/util/SensorConfigUtil.hpp"
#include "cybertron/glm/function.hpp"

CYBERTRON_BEGIN

bool SensorConfigUtil::loadConfig(const cybertron::json& jsonData, SensorGpsImuConfig& config)
{
	if (JsonReader::hasValue(jsonData, "params")) {
		auto jsonParams = JsonReader::getChild(jsonData, "params");
		if (JsonReader::hasValue(jsonData, "x")) config.Position.x = (float)JsonReader::getDouble(jsonData, "x");
		if (JsonReader::hasValue(jsonData, "y")) config.Position.y = (float)JsonReader::getDouble(jsonData, "y");
		if (JsonReader::hasValue(jsonData, "z")) config.Position.z = (float)JsonReader::getDouble(jsonData, "z");
		if (JsonReader::hasValue(jsonData, "roll")) config.Rotation.x = (float)JsonReader::getDouble(jsonData, "roll");
		if (JsonReader::hasValue(jsonData, "pitch")) config.Rotation.y = (float)JsonReader::getDouble(jsonData, "pitch");
		if (JsonReader::hasValue(jsonData, "yaw")) config.Rotation.z = (float)JsonReader::getDouble(jsonData, "yaw");

		if (JsonReader::hasValue(jsonParams, "frequency")) config.HzImu = (float)JsonReader::getDouble(jsonParams, "frequency");
		if (JsonReader::hasValue(jsonParams, "hzGps")) config.HzGps = (float)JsonReader::getDouble(jsonParams, "hzGps");
		if (JsonReader::hasValue(jsonParams, "gravity")) config.Gravity = (float)JsonReader::getDouble(jsonParams, "gravity");
		if (JsonReader::hasValue(jsonParams, "gpsSignal")) config.GpsSignal = (float)JsonReader::getDouble(jsonParams, "gpsSignal");
		if (JsonReader::hasValue(jsonParams, "positionError")) {
			auto jsonError = JsonReader::getChild(jsonParams, "positionError");
			if (JsonReader::hasValue(jsonError, "horizontalPositionError")) config.HorizontalPositionError = (float)JsonReader::getDouble(jsonError, "horizontalPositionError");
			if (JsonReader::hasValue(jsonError, "verticalPositionError")) config.VerticalPositionError = (float)JsonReader::getDouble(jsonError, "verticalPositionError");
			if (JsonReader::hasValue(jsonError, "attitudeError")) config.AttitudeError = degreeToRadian((float)JsonReader::getDouble(jsonError, "attitudeError"));
			if (JsonReader::hasValue(jsonError, "velocityError")) config.VelocityError = (float)JsonReader::getDouble(jsonError, "velocityError");
			if (JsonReader::hasValue(jsonError, "accelerationError")) config.AccelerationError = (float)JsonReader::getDouble(jsonError, "accelerationError");
			if (JsonReader::hasValue(jsonError, "angularVelocityError")) config.AngularVelocityError = degreeToRadian((float)JsonReader::getDouble(jsonError, "angularVelocityError"));
		}

		return true;
	}
	return false;
}

// customized hash to make it unique across windows and ubuntu.
namespace SimOneHash
{
	static size_t hash(const char*__ptr, size_t __clength)
	{
		size_t __result = static_cast<size_t>(14695981039346656037ULL);
		const char *__cptr = reinterpret_cast<const char *>(__ptr);
		for (; __clength; --__clength)
		{
			__result ^= static_cast<size_t>(*__cptr++);
			__result *= static_cast<size_t>(1099511628211ULL);
		}
		return __result;
	}
	static size_t hash(std::uint32_t val)
	{
		return SimOneHash::hash((const char*)&val, sizeof(val));
	}
};

std::uint32_t SensorConfigUtil::getHashValue(const std::string& hashStr) {
	if (!hashStr.empty()) {
		return std::uint32_t(SimOneHash::hash(hashStr.c_str(), hashStr.size()) & 0xffffff);
	}
	return 0;
}
std::uint32_t SensorConfigUtil::getHashValue(std::uint32_t trafficFlowActorId, std::uint32_t trafficFlowActorType/* = 0*/)
{
	static std::hash<std::uint32_t> actorHash;
	if (trafficFlowActorId > 0) {
		std::uint32_t hashResult = SimOneHash::hash(trafficFlowActorId) & 0xffffff;
		if (trafficFlowActorType == 17) { // For rider, don't use the same id with bicycle/motorcycle
			hashResult = ~hashResult & 0xffffff;
		}
		return hashResult;
	}
	return 0;
}

CYBERTRON_END